#include "workingstate.h"
#include "fsm.h"
#include "maincontroller.h"
#include "masters/vehiclecontroller.h"
#include "device/dhdc.h"
#include "device/drdc.h"
using namespace std;
using namespace vehicle;
using namespace Eigen;
WorkingState::WorkingState()
{

}

/**
 * @brief ForceState::Update
 * @param fsm
 *
 */

void WorkingState::Update(FSM *fsm)
{
//    static int i=0;
//    cout<<i<<" :force state"<<endl;

    switch (fsm->vehicle_controller_->GetRunningStatus())
    {
    case main_controller::kRCPause:
        fsm->SetState(BaseState::kPauseState);
        break;
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
    {
        char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();

        ushort sn=0;
        int ret=dhdGetSerialNumber(&sn,master_device);
        if(ret!=0)
        {
            fsm->vehicle_controller_->SetMasterDeviceIndex(-1);

            master_device=-1;
        }


        if(master_device<0)
            fsm->SetState(BaseState::kPauseState);
        else
        {
            if(fsm->vehicle_controller_->GetConnectionStatus()==main_controller::kConnection2RobotLost)
                fsm->SetState(BaseState::kPauseState);
            else
            {
                double px,py,pz;
                dhdGetPosition(&px,&py,&pz,master_device);

                double tar_v=px;
                double tar_w=py;
                fsm->vehicle_controller_->SendTarget2Slave(tar_v,tar_w);

                pair<double,double> vel=fsm->vehicle_controller_->GetVehicleCurVel();
                double cur_v=vel.first;
                double cur_w=vel.second;
                dhdSetForce(-(tar_v-cur_v),-(tar_w-cur_w),-pz,master_device);
            }
        }
    }
        break;
    default:
        break;
    }

}

void WorkingState::OnEntry(FSM *fsm)
{
    cout<<"entry force state"<<endl;
    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();
    if(master_device<0)
        return;

    drdRegulatePos(false,master_device);
    drdRegulateGrip(false,master_device);

    dhdSetForceAndGripperForce(0,0,0,0,master_device);

}

void WorkingState::OnExit(FSM *fsm)
{
    cout<<"exit force state"<<endl;
    fsm->vehicle_controller_->SendTarget2Slave(0,0);
//    if(fsm->vehicle_controller_->SetMasterForce(Eigen::Vector3d::Zero()))
//        cout<<"failed to clear feedback force"<<endl;

}
